
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_airspeed_param.c
  * @author     baiyang
  * @date       2023-8-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_airspeed.h"

#include <parameter/param.h>
#include <vehicle/vehicle_type.h>
/*-----------------------------------macro------------------------------------*/
#ifdef HAL_AIRSPEED_TYPE_DEFAULT
 #define ARSPD_DEFAULT_TYPE HAL_AIRSPEED_TYPE_DEFAULT
 #ifndef ARSPD_DEFAULT_PIN
 #define ARSPD_DEFAULT_PIN 1
 #endif
#elif MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
 // The HAL_BOARD_SITL setting is required because of current probe process for MS4525 will
 // connect and find the SIM_DLVR sensors & fault as there is no way to tell them apart
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL_WIN
  #define ARSPD_DEFAULT_TYPE AS_TYPE_ANALOG
  #define ARSPD_DEFAULT_PIN 1
 #else
  #define ARSPD_DEFAULT_TYPE AS_TYPE_I2C_MS4525
  #ifdef HAL_DEFAULT_AIRSPEED_PIN
      #define ARSPD_DEFAULT_PIN HAL_DEFAULT_AIRSPEED_PIN
  #else
     #define ARSPD_DEFAULT_PIN 15
  #endif
 #endif //CONFIG_HAL_BOARD
#else   // All Other Vehicle Types
 #define ARSPD_DEFAULT_TYPE AS_TYPE_NONE
 #define ARSPD_DEFAULT_PIN 15
#endif

#ifndef HAL_AIRSPEED_BUS_DEFAULT
#define HAL_AIRSPEED_BUS_DEFAULT 1
#endif

#define ENABLE_PARAMETER !(MB_BUILD_TYPE(MICROBEE_BUILD_Plane))

#define OPTIONS_DEFAULT (AS_OM_ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE | AS_OM_ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE | AS_OM_USE_EKF_CONSISTENCY)

#ifndef PSI_RANGE_DEFAULT
#define PSI_RANGE_DEFAULT 1.0f
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    // @Param: _ENABLE
    // @DisplayName: Airspeed Enable
    // @Description: Enable airspeed sensor support
    // @Values: 0:Disable, 1:Enable
    // @User: Standard
    PARAM_DEFINE_INT8(ARSPD_ENABLE, 0),

#if AIRSPEED_MAX_SENSORS > 1
    // @Param: _PRIMARY
    // @DisplayName: Primary airspeed sensor
    // @Description: This selects which airspeed sensor will be the primary if multiple sensors are found
    // @Values: 0:FirstSensor,1:2ndSensor
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_PRIMARY, 0),
#endif

    // @Param: _OPTIONS
    // @DisplayName: Airspeed options bitmask
    // @Description: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction, 3:Check that the airspeed is statistically consistent with the navigation EKF vehicle and wind velocity estimates using EKF3 (requires AHRS_EKF_TYPE = 3)
    // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
    // @Bitmask: 0:SpeedMismatchDisable, 1:AllowSpeedMismatchRecovery, 2:DisableVoltageCorrection, 3:UseEkf3Consistency
    // @User: Advanced
    PARAM_DEFINE_INT32(ARSPD_OPTIONS, OPTIONS_DEFAULT),

    // @Param: _WIND_MAX
    // @DisplayName: Maximum airspeed and ground speed difference
    // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will be marked unhealthy. Using ARSPD_OPTION this health value can be used to disable the sensor.
    // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
    // @Units: m/s
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_WIND_MAX, 0),

    // @Param: _WIND_WARN
    // @DisplayName: Airspeed and GPS speed difference that gives a warning
    // @Description: If the difference between airspeed and GPS speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used.
    // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
    // @Units: m/s
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_WIND_WARN, 0),

    // @Param: _WIND_GATE
    // @DisplayName: Re-enable Consistency Check Gate Size
    // @Description: Number of standard deviations applied to the re-enable EKF consistency check that is used when ARSPD_OPTIONS bit position 3 is set. Larger values will make the re-enabling of the airspeed sensor faster, but increase the likelihood of re-enabling a degraded sensor. The value can be tuned by using the ARSP.TR log message by setting ARSPD_WIND_GATE to a value that is higher than the value for ARSP.TR observed with a healthy airspeed sensor. Occasional transients in ARSP.TR above the value set by ARSPD_WIND_GATE can be tolerated provided they are less than 5 seconds in duration and less than 10% duty cycle.
    // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle.
    // @Range: 0.0 10.0
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_WIND_GATE, 5.0f),

    // @Param: _OFF_PCNT
    // @DisplayName: Maximum offset cal speed error 
    // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibraions before a warning is issued. This potential speed error is in percent of ASPD_FBW_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
    // @Range: 0.0 10.0
    // @Units: %
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_OFF_PCNT, 0),

    // @Param: TYPE
    // @DisplayName: Airspeed type
    // @Description: Type of airspeed sensor
    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL
    // @User: Standard
    PARAM_DEFINE_INT8(ARSPD_TYPE, ARSPD_DEFAULT_TYPE),

    // @Param: USE
    // @DisplayName: Airspeed use
    // @Description: Enables airspeed use for automatic throttle modes and replaces control from THR_TRIM. Continues to display and log airspeed if set to 0. Uses airspeed for control if set to 1. Only uses airspeed when throttle = 0 if set to 2 (useful for gliders with airspeed sensors behind propellers).
    // @Description{Copter, Blimp, Rover, Sub}: This parameter is not used by this vehicle. Always set to 0.
    // @Values: 0:DoNotUse,1:Use,2:UseWhenZeroThrottle
    // @User: Standard
    PARAM_DEFINE_INT8(ARSPD_USE, 0),

    // @Param: OFFSET
    // @DisplayName: Airspeed offset
    // @Description: Airspeed calibration offset
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_OFFSET, 0),

    // @Param: RATIO
    // @DisplayName: Airspeed ratio
    // @Description: Calibrates pitot tube pressure to velocity. Increasing this value will indicate a higher airspeed at any given dynamic pressure.
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_RATIO, 2),

    // @Param: PIN
    // @DisplayName: Airspeed pin
    // @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port. 
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_PIN, ARSPD_DEFAULT_PIN),

#if MB_AIRSPEED_AUTOCAL_ENABLE
    // @Param: AUTOCAL
    // @DisplayName: Automatic airspeed ratio calibration
    // @DisplayName{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
    // @Description: Enables automatic adjustment of airspeed ratio during a calibration flight based on estimation of ground speed and true airspeed. New ratio saved every 2 minutes if change is > 5%. Should not be left enabled.
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_AUTOCAL, 0),
#endif

    // @Param: TUBE_ORDR
    // @DisplayName: Control pitot tube order
    // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
    // @User: Advanced
    // @Values: 0:Normal,1:Swapped,2:Auto Detect
    PARAM_DEFINE_INT8(ARSPD_TUBE_ORDR, 2),

    // @Param: SKIP_CAL
    // @DisplayName: Skip airspeed offset calibration on startup
    // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot.
    // @Values: 0:Disable,1:Enable
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_SKIP_CAL, 0),

    // @Param: PSI_RANGE
    // @DisplayName: The PSI range of the device
    // @Description: This parameter allows you to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD_PSI_RANGE, PSI_RANGE_DEFAULT),

    // @Param: BUS
    // @DisplayName: Airspeed I2C bus
    // @Description: Bus number of the I2C bus where the airspeed sensor is connected. May not correspond to board's I2C bus number labels. Retry another bus and reboot if airspeed sensor fails to initialize.
    // @Values: 0:Bus0,1:Bus1,2:Bus2
    // @RebootRequired: True
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD_BUS, HAL_AIRSPEED_BUS_DEFAULT),

    // @Param: DEVID
    // @DisplayName: Airspeed ID
    // @Description: Airspeed sensor ID, taking into account its type, bus and instance
    // @ReadOnly: True
    // @User: Advanced
    PARAM_DEFINE_INT32(ARSPD_DEVID, 0),

#if AIRSPEED_MAX_SENSORS > 1
    // @Param: TYPE
    // @DisplayName: Airspeed type
    // @Description: Type of airspeed sensor
    // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77),6:I2C-SDP3X,7:I2C-DLVR-5in,8:DroneCAN,9:I2C-DLVR-10in,10:I2C-DLVR-20in,11:I2C-DLVR-30in,12:I2C-DLVR-60in,13:NMEA water speed,14:MSP,15:ASP5033,100:SITL
    // @User: Standard
    PARAM_DEFINE_INT8(ARSPD2_TYPE, 0),

    // @Param: USE
    // @DisplayName: Airspeed use
    // @Description: Enables airspeed use for automatic throttle modes and replaces control from THR_TRIM. Continues to display and log airspeed if set to 0. Uses airspeed for control if set to 1. Only uses airspeed when throttle = 0 if set to 2 (useful for gliders with airspeed sensors behind propellers).
    // @Description{Copter, Blimp, Rover, Sub}: This parameter is not used by this vehicle. Always set to 0.
    // @Values: 0:DoNotUse,1:Use,2:UseWhenZeroThrottle
    // @User: Standard
    PARAM_DEFINE_INT8(ARSPD2_USE, 0),

    // @Param: OFFSET
    // @DisplayName: Airspeed offset
    // @Description: Airspeed calibration offset
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD2_OFFSET, 0),

    // @Param: RATIO
    // @DisplayName: Airspeed ratio
    // @Description: Calibrates pitot tube pressure to velocity. Increasing this value will indicate a higher airspeed at any given dynamic pressure.
    // @Increment: 0.1
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD2_RATIO, 2),

    // @Param: PIN
    // @DisplayName: Airspeed pin
    // @Description: The pin number that the airspeed sensor is connected to for analog sensors. Set to 15 on the Pixhawk for the analog airspeed port. 
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD2_PIN, 0),

#if MB_AIRSPEED_AUTOCAL_ENABLE
    // @Param: AUTOCAL
    // @DisplayName: Automatic airspeed ratio calibration
    // @DisplayName{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
    // @Description: Enables automatic adjustment of airspeed ratio during a calibration flight based on estimation of ground speed and true airspeed. New ratio saved every 2 minutes if change is > 5%. Should not be left enabled.
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD2_AUTOCAL, 0),
#endif

    // @Param: TUBE_ORDR
    // @DisplayName: Control pitot tube order
    // @Description: This parameter allows you to control whether the order in which the tubes are attached to your pitot tube matters. If you set this to 0 then the first (often the top) connector on the sensor needs to be the stagnation pressure (the pressure at the tip of the pitot tube). If set to 1 then the second (often the bottom) connector needs to be the stagnation pressure. If set to 2 (the default) then the airspeed driver will accept either order. The reason you may wish to specify the order is it will allow your airspeed sensor to detect if the aircraft is receiving excessive pressure on the static port compared to the stagnation port such as during a stall, which would otherwise be seen as a positive airspeed.
    // @User: Advanced
    // @Values: 0:Normal,1:Swapped,2:Auto Detect
    PARAM_DEFINE_INT8(ARSPD2_TUBE_ORDR, 2),

    // @Param: SKIP_CAL
    // @DisplayName: Skip airspeed offset calibration on startup
    // @Description: This parameter allows you to skip airspeed offset calibration on startup, instead using the offset from the last calibration. This may be desirable if the offset variance between flights for your sensor is low and you want to avoid having to cover the pitot tube on each boot.
    // @Values: 0:Disable,1:Enable
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD2_SKIP_CAL, 0),

    // @Param: PSI_RANGE
    // @DisplayName: The PSI range of the device
    // @Description: This parameter allows you to set the PSI (pounds per square inch) range for your sensor. You should not change this unless you examine the datasheet for your device
    // @User: Advanced
    PARAM_DEFINE_FLOAT(ARSPD2_PSI_RANGE, PSI_RANGE_DEFAULT),

    // @Param: BUS
    // @DisplayName: Airspeed I2C bus
    // @Description: Bus number of the I2C bus where the airspeed sensor is connected. May not correspond to board's I2C bus number labels. Retry another bus and reboot if airspeed sensor fails to initialize.
    // @Values: 0:Bus0,1:Bus1,2:Bus2
    // @RebootRequired: True
    // @User: Advanced
    PARAM_DEFINE_INT8(ARSPD2_BUS, 1),

    // @Param: DEVID
    // @DisplayName: Airspeed ID
    // @Description: Airspeed sensor ID, taking into account its type, bus and instance
    // @ReadOnly: True
    // @User: Advanced
    PARAM_DEFINE_INT32(ARSPD2_DEVID, 0),
#endif
};
PARAM_GROUP_DEFINE(ARSPD, var_info);
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_as_assign_param()
{
    sensor_airspeed_t snsr_as = sensor_as_get_singleton();

#if ENABLE_PARAMETER
    param_link_variable(PARAM_ID(ARSPD, ARSPD_ENABLE), &snsr_as->_enable);
#endif
    param_link_variable(PARAM_ID(ARSPD, ARSPD_PRIMARY), &snsr_as->primary_sensor);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_OPTIONS), &snsr_as->_options);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_WIND_MAX), &snsr_as->_wind_max);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_WIND_WARN), &snsr_as->_wind_warn);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_WIND_GATE), &snsr_as->_wind_gate);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_OFF_PCNT), &snsr_as->max_speed_pcnt);

    param_link_variable(PARAM_ID(ARSPD, ARSPD_TYPE), &snsr_as->param[0].type);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_USE), &snsr_as->param[0].use);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_OFFSET), &snsr_as->param[0].offset);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_RATIO), &snsr_as->param[0].ratio);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_PIN), &snsr_as->param[0].pin);
#if MB_AIRSPEED_AUTOCAL_ENABLE
    param_link_variable(PARAM_ID(ARSPD, ARSPD_AUTOCAL), &snsr_as->param[0].autocal);
#endif
    param_link_variable(PARAM_ID(ARSPD, ARSPD_TUBE_ORDR), &snsr_as->param[0].tube_order);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_SKIP_CAL), &snsr_as->param[0].skip_cal);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_PSI_RANGE), &snsr_as->param[0].psi_range);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_BUS), &snsr_as->param[0].bus);
    param_link_variable(PARAM_ID(ARSPD, ARSPD_DEVID), &snsr_as->param[0].bus_id);

#if AIRSPEED_MAX_SENSORS > 1
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_TYPE), &snsr_as->param[1].type);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_USE), &snsr_as->param[1].use);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_OFFSET), &snsr_as->param[1].offset);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_RATIO), &snsr_as->param[1].ratio);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_PIN), &snsr_as->param[1].pin);
#if MB_AIRSPEED_AUTOCAL_ENABLE
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_AUTOCAL), &snsr_as->param[1].autocal);
#endif
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_TUBE_ORDR), &snsr_as->param[1].tube_order);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_SKIP_CAL), &snsr_as->param[1].skip_cal);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_PSI_RANGE), &snsr_as->param[1].psi_range);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_BUS), &snsr_as->param[1].bus);
    param_link_variable(PARAM_ID(ARSPD, ARSPD2_DEVID), &snsr_as->param[1].bus_id);
#endif
}

/*------------------------------------test------------------------------------*/


